import math
# author = "Ronald"
# date   = 24-04-2010 07:29:02 PM

from OpenGL.GL import *
from OpenGL.GL import glScale
from OpenGL.GLU import *
from OpenGL.GLUT import *
from numpy import *
from leftArm import LeftArm
from rightArm import RightArm
from body import Body
from leg import Leg

runExtension = 8.0
runSpeed = 5.0
runForwardSpeed = 0.2
moonWalkExtension = 6.0
moonWalkSpeed = 4
moonWalkBackSpeed = 0.15
steps = 0.0

class Robot(object):

    def __init__(self, size, position):
        self.size = size
        self.position = position
        self.angle = 0
        self.leftArm = LeftArm(0.5, [-1.5, 1.5, 0])
        self.rightArm = RightArm(0.5, [1.5, 1.5, 0])
        self.body = Body(4, [0,0,0])
        self.rightLeg = Leg(0.5, [-0.2, -3.0, 0])
        self.leftLeg = Leg(0.5, [1.2, -3.0, 0])


    def draw(self):

        glPushMatrix()
        glTranslatef (self.position[0], self.position[1], self.position[2])
        glRotate(self.angle,0,1,0)
        glScalef(self.size, self.size, self.size)
        self.rightArm.draw()
        self.leftArm.draw()
        self.body.draw()
#        self.leg.draw()
        self.leftLeg.draw()
        self.rightLeg.draw()
        if self.rightArm.bulletBool == 1 :
            self.rightArm.bulletShoot()
        glPopMatrix()


    #funciones de movimiento del robot
    
    def angleAdd(self):
        self.angle = self.angle + 2

    def angleSubstract(self):
        self.angle = self.angle - 2

    def positionAdd(self,speed):
        self.position[0] = self.position[0] + sin(self.angle*math.pi/180)*speed
        self.position[2] = self.position[2] + cos(self.angle*math.pi/180)*speed

    def positionSubstract(self,speed):
        self.position[0] = self.position[0] - sin(self.angle*math.pi/180)*speed
        self.position[2] = self.position[2] - cos(self.angle*math.pi/180)*speed

    def run(self):
        global steps, runExtension

        #los movimientos de caminar divididos en 6, incluido el avance
        if (steps/runExtension)%6 < 1 :
            self.leftLeg.runFront(runSpeed)                     #movimiento de las piernas
            self.rightLeg.runBack(runSpeed)
            self.leftArm.forearmYSubstract()            #movimiento del brazo izquierdo
            self.leftArm.armYSubstract()
            self.position[1] = self.position[1] + 0.05   #movimiento (salto) en el eje y
            self.positionAdd(runForwardSpeed)

        if ((steps/runExtension)%6 >= 1) and ((steps/runExtension)%6 < 2) :
            self.leftLeg.runFront2(runSpeed)
            self.rightLeg.runBack2(runSpeed)
            self.leftArm.forearmYSubstract()
            if steps % 2 == 0 :
                self.leftArm.armYAdd()
            if ((steps/runExtension)%6 < 1.5) :                #movimiento (salto) en eje y
                self.position[1] = self.position[1] + 0.03
            else :
                self.position[1] = self.position[1] - 0.03
            self.positionAdd(runForwardSpeed)

        if ((steps/runExtension)%6 >= 2) and ((steps/runExtension)%6 < 3) :
            self.leftLeg.runFront3(runSpeed)
            self.rightLeg.runBack3(runSpeed)
            self.leftArm.forearmYSubstract()
            self.leftArm.armYAdd()
            self.position[1] = self.position[1] - 0.05
            self.positionAdd(runForwardSpeed)

        if ((steps/runExtension)%6 >= 3) and ((steps/runExtension)%6 < 4) :
            self.leftLeg.runBack(runSpeed)
            self.rightLeg.runFront(runSpeed)
            self.leftArm.forearmYSubstract()
            self.leftArm.armYAdd()
            self.position[1] = self.position[1] + 0.05
            self.positionAdd(runForwardSpeed)

        if ((steps/runExtension)%6 >= 4) and ((steps/runExtension)%6 < 5) :
            self.leftLeg.runBack2(runSpeed)
            self.rightLeg.runFront2(runSpeed)
            self.leftArm.forearmYSubstract()
            if steps % 2 != 0 :
                self.leftArm.armYSubstract()
            if ((steps/runExtension)%6 < 4.5) :                #movimiento (salto) en eje y
                self.position[1] = self.position[1] + 0.03
            else :
                self.position[1] = self.position[1] - 0.03
            self.positionAdd(runForwardSpeed)

        if ((steps/runExtension)%6 >= 5) :
            self.leftLeg.runBack3(runSpeed)
            self.rightLeg.runFront3(runSpeed)
            self.leftArm.forearmYSubstract()
            self.leftArm.armYSubstract()
            self.position[1] = self.position[1] - 0.05
            self.positionAdd(runForwardSpeed)

        steps = steps + 1

    def transform(self):
        self.leftArm.armYAdd()
        self.rightArm.armYAdd()



#    def positionXAdd(self):h
#        self.position[0] = self.position[0] + 0.1
#
#    def positionXSubstract(self):
#        self.position[0] = self.position[0] - 0.1
#
#    def positionYAdd(self):
#        self.position[1] = self.position[1] + 0.1
#
#    def positionYSubstract(self):
#        self.position[1] = self.position[1] - 0.1
#
#    def positionZAdd(self):
#        self.position[2] = self.position[2] - 0.1
#
#    def positionZSubstract(self):
#        self.position[2] = self.position[2] + 0.1
    def moonwalk(self):
        global steps, moonWalkExtension
#        extension2 = extension - 4

        #los movimientos de caminar divididos en 6, incluido el avance
        if (steps/moonWalkExtension)%6 < 1 :
            self.leftLeg.runFront(moonWalkSpeed)                     #movimiento de las piernas
            self.rightLeg.runBack(moonWalkSpeed)
            # self.leftArm.forearmYSubstract()            #movimiento del brazo izquierdo
            # self.leftArm.armYSubstract()
            # self.position[1] = self.position[1] + 0.05   #movimiento (salto) en el eje y
            self.positionSubstract(moonWalkBackSpeed)

        if ((steps/moonWalkExtension)%6 >= 1) and ((steps/moonWalkExtension)%6 < 2) :
            self.leftLeg.runFront2(moonWalkSpeed)
            self.rightLeg.runBack2(moonWalkSpeed)
            #self.leftArm.forearmYSubstract()
            #if steps % 2 == 0 :
            #    self.leftArm.armYAdd()
            #if ((steps/moonWalkExtension)%6 < 1.5) :                #movimiento (salto) en eje y
            #    self.position[1] = self.position[1] + 0.03
            #else :
            #    self.position[1] = self.position[1] - 0.03
            self.positionSubstract(moonWalkBackSpeed)

        if ((steps/moonWalkExtension)%6 >= 2) and ((steps/moonWalkExtension)%6 < 3) :
            self.leftLeg.runFront3(moonWalkSpeed)
            self.rightLeg.runBack3(moonWalkSpeed)
            #self.leftArm.forearmYSubstract()
            #self.leftArm.armYAdd()
            #self.position[1] = self.position[1] - 0.05
            self.positionSubstract(moonWalkBackSpeed)

        if ((steps/moonWalkExtension)%6 >= 3) and ((steps/moonWalkExtension)%6 < 4) :
            self.leftLeg.runBack(moonWalkSpeed)
            self.rightLeg.runFront(moonWalkSpeed)
            #self.leftArm.forearmYSubstract()
            #self.leftArm.armYAdd()
            #self.position[1] = self.position[1] + 0.05
            self.positionSubstract(moonWalkBackSpeed)

        if ((steps/moonWalkExtension)%6 >= 4) and ((steps/moonWalkExtension)%6 < 5) :
            self.leftLeg.runBack2(moonWalkSpeed)
            self.rightLeg.runFront2(moonWalkSpeed)
            #self.leftArm.forearmYSubstract()
            #if steps % 2 != 0 :
            #    self.leftArm.armYSubstract()
            #if ((steps/moonWalkExtension)%6 < 4.5) :                #movimiento (salto) en eje y
            #    self.position[1] = self.position[1] + 0.03
            #else :
            #    self.position[1] = self.position[1] - 0.03
            self.positionSubstract(moonWalkBackSpeed)

        if ((steps/moonWalkExtension)%6 >= 5) :
            self.leftLeg.runBack3(moonWalkSpeed)
            self.rightLeg.runFront3(moonWalkSpeed)
            #self.leftArm.forearmYSubstract()
            #self.leftArm.armYSubstract()
            #self.position[1] = self.position[1] - 0.05
            self.positionSubstract(moonWalkBackSpeed)

        steps = steps + 1

    def restorePosition(self):
        if self.position[0] < 0 :
            self.position[0] = self.position[0] + 0.01
        elif self.position[0] > 0 :
            self.position[0] = self.position[0] - 0.01
        if self.position[1] < -0.56 :
            self.position[1] = self.position[1] + 0.01
        elif self.position[1] > -0.56 :
            self.position[1] = self.position[1] - 0.01
        if self.position[2] < 0 :
            self.position[2] = self.position[2] + 0.01
        elif self.position[2] > 0 :
            self.position[2] = self.position[2] - 0.01